#include <ICM20948_WE.h>
#include <Arduino.h>
#include <WiFi.h>
#include <ESP32Servo.h>

#define SERIAL_PORT Serial
#define PI 3.14159265358979323846
#define FUYANG
#define GUNZHUAN
//#undef FUYANG
#undef GUNZHUAN
float roll=0, pitch=90, yaw;
double aimRoll = 0;
double droll = 0;
double aimpitch = 90;
double dpitch = 0;
double imu_x,imu_y,imu_z;
double can1,can2=50,can3,can4=50;
double can_roll=-10;
// 滤波后的Pitch、Roll、Yaw
float filteredPitch = 90;
float filteredRoll = 0;
float filteredYaw = 0;
int finalspeed1,finalspeed2;
Servo myservo1;  // J1,电机
Servo myservo2;  // J2，电机
Servo myservo3;  // J3,舵机
Servo myservo4;  // J4，舵机
const float alpha = 0.95;
float pitch_acc;//加速度解算出的角度
float roll_acc;//
float pitch_gyro;
float roll_gyro;
float yaw_gyro;//陀螺仪积分出来的yaw角度
float pitch_setup_data=0;//加速度计解算的初始姿态，用于给陀螺仪积分做初始基准
float roll_setup_data=0;
// 姿态角度变量

unsigned long prev_time = 0;
int servo_pin1 = 38; //      
int servo_pin2 = 45; //
int servo_pin3 = 48; //
int servo_pin4 = 47; //


unsigned long gzcurrentTime, gzpreviousTime;
double gzelapsedTime;
double gzerror1;
double gzlastError1;
double gzcumError1, gzrateError1;
double gzkp = 1.2;
double gzki = 0.00001;
double gzkd = 0.5;
const double antiWindupThreshold3 = 20; // 积分项限制阈值

unsigned long fycurrentTime, fypreviousTime;
double fyelapsedTime;
double fyerror1;
double fylastError1;
double fycumError1, fyrateError1;
double fykp = 0.6;
double fyki = 0.00001;
double fykd = 0.001;
//动补数据获取
double dbx=0;
double dby=0;
double dbz=0;
const double antiWindupThreshold2 = 20; // 积分项限制阈值

double pos2 = 0;    //pitch
double pos1 = 0;    //pitch
double num1=0;
double num2=0;
int dtime=10;

const int windowSize = 5;
float dataWindow[windowSize];
int dataIndex = 0;

float threshold = 150;  // 倍值，根据实际情况调整
float lastValidValue = 10;
// 倍值滤波函数

// 中值滤波函数
float medianFilter(float rawValue) {
  // 将新数据加入窗口
  dataWindow[dataIndex] = rawValue;
  dataIndex = (dataIndex + 1) % windowSize;

  // 复制数据窗口并排序
  float sortedWindow[windowSize];
  for (int i = 0; i < windowSize; i++) {
    sortedWindow[i] = dataWindow[i];
  }
  for (int i = 0; i < windowSize - 1; i++) {
    for (int j = i + 1; j < windowSize; j++) {
      if (sortedWindow[i] > sortedWindow[j]) {
        float temp = sortedWindow[i];
        sortedWindow[i] = sortedWindow[j];
        sortedWindow[j] = temp;
      }
    }
  }

  // 返回中值
  return sortedWindow[windowSize / 2];
}

float thresholdFilter(float rawValue) {
  // 计算当前数据点与上一个有效数据点的差值
  float delta = abs(rawValue/lastValidValue);

  // 如果倍值超过阈值，认为是毛刺，返回上五个值的均值
  if (delta > threshold) {
    float median = medianFilter(rawValue);
    lastValidValue = median;
    return median;
  }
  else {
  return rawValue;
  }
  // 否则，更新上一个有效数据点并返回当前值
  lastValidValue = rawValue;
}


double gzPID(double gzSetpoint, double inp){
        gzcurrentTime = millis();                //get current time
        gzelapsedTime = (double)(gzcurrentTime - gzpreviousTime);        //compute time elapsed from previous computation
        
        gzerror1 = gzSetpoint - inp;                                // determine error
        gzcumError1 += gzerror1 * gzelapsedTime;                // compute integral
        if (gzcumError1 > antiWindupThreshold3) {
          gzcumError1 = antiWindupThreshold3;
        }
        if (gzcumError1 < -antiWindupThreshold3) {
          gzcumError1 = -antiWindupThreshold3;
        }        
        gzrateError1 = (gzerror1 - gzlastError1)/gzelapsedTime;   // compute derivative 
        double gzout1 = gzkp*gzerror1 + gzki*gzcumError1 + gzkd*gzrateError1;                //PID output  
        return gzout1;
}
double fyPID(double fySetpoint, double inp){
        fycurrentTime = millis();                //get current time
        fyelapsedTime = (double)(fycurrentTime - fypreviousTime);        //compute time elapsed from previous computation
        
        fyerror1 = fySetpoint - inp;                                // determine error
        fycumError1 += fyerror1 * fyelapsedTime;                // compute integral
        if (fycumError1 > antiWindupThreshold2) {
          fycumError1 = antiWindupThreshold2;
        }
        if (fycumError1 < -antiWindupThreshold2) {
          fycumError1 = -antiWindupThreshold2;
        }        
        fyrateError1 = (fyerror1 - fylastError1)/fyelapsedTime;   // compute derivative 
        double fyout1 = fykp*fyerror1 + fyki*fycumError1 + fykd*fyrateError1;                //PID output  
        return fyout1;
}
void print_cun(double out1,double out2,double out3,double out4) {
    // Serial.print("DATA,");
    // Serial.print( out1 );
    // Serial.print(",");
    // Serial.print( out2 );
    // Serial.print(",");
    // Serial.print( out3 );
    // Serial.print(",");
    // Serial.println( out4 );
}
void print_gz(double aimroll, double droll, double Roll) {
    // Serial.print("aimroll:");
    // Serial.print( aimroll );
    // Serial.print("droll:");
    // Serial.print( droll );
    // Serial.print(",");
    // Serial.print("Roll:");
    // Serial.println( Roll );
}

//校准后的参数输出
void print_calibration() {
      // SERIAL_PORT.print(F("Roll:"));
      // SERIAL_PORT.print(roll, 1);
      // SERIAL_PORT.print(F(" Pitch:"));
      // SERIAL_PORT.print(pitch, 1);
      // SERIAL_PORT.print(F(" Yaw:"));
      // SERIAL_PORT.println(yaw, 1);
}
int limit(float num,float min,float max){
  if(num<min){
    num=min;
  }else if(num>max){
    num=max;
  }
  return num;
}